#include <bwio.h>
#include <io.h>
#include <user.h>
#include <defs.h>
#include <ts7200.h>
#include <commands.h>

void NameServer();
void SyncServer();
void ClockServer();
void TrainRxServer();
void TrainTxServer();
void TerminalRxServer();
void TerminalTxServer();
void IdleTask();

int SensorEqual( sensor *ss1, sensor *ss2 ) {
  return (ss1->set == ss2->set) && (ss1->id == ss2->id);
}

void SensorInit( sensor *ss, char set, char b1, char b2 ) {
  ss->set = set;
  unsigned short val = (b1 << 8) | b2;
  int bit = 16;
  while (!(val & 1) && bit > 0) {
    val >>= 1;
    bit--;
  }
  ss->id = bit;
}

void PrintData(char *msg) {
  int i;
  for (i = 0; i < 10; i++) {
    Printf("%x ", msg[i]);
  }
  Printf("\r\n");
}

void SensorMonitor() {
  int diff, c, i;
  char last[10];
  for (i = 0; i < 10; i++)
    last[i] = 0x00;
  char curr[10];
  char msg[10];
  
  int driver_tid;
  Receive(&driver_tid, NULL, 0);
  Reply(driver_tid, NULL, 0);

  for ( ; ; ) {
    Delay(5);
    AcquireLock(SECTION_TRAIN_TX);
    Putc( COM1, TRAIN_SENSORS_BASE + NUM_SENSOR_MODULES );
    ReleaseLock(SECTION_TRAIN_TX);
    diff = FALSE;
    for (i = 0; i < 10; i++) {
      c = Getc(COM1);
      curr[i] = c;
      msg[i] = curr[i] & ~last[i];
      if (msg[i])
        diff = TRUE;
    }
    if (diff) {
      PrintData(msg);
      Send(driver_tid, msg, 10, NULL, 0);
    }
    MemCopy(last, curr, 10);
  }
  Exit();
}

int Timer4Read() {
  int *timer4_lval = (int*)( TIMER4_LVAL );
  return *timer4_lval / TIMER4_TICKS_PER_MS;
}

void DataPrint( track *sts, sensor *ss, int train_number) {
  int train_speed = sts->tr_speed[train_number];
  int timestamp = Timer4Read();
  
  Printf("%d %d %c%d %d\r\n",
      train_number, train_speed, ss->set, ss->id, timestamp );
  int i;
}

void FillSensorData(char *msg, sensor *data) {
  int last = -1;
  int i;
  for (i = 0; i < 5; i++) {
    if (msg[2*i] || msg[2*i+1])
      last = i;
  }
  SensorInit(data, 'A'+last, msg[2*last], msg[2*last+1]);
}

void TrainDriver() {
  int i;
  int N = 11;   // number of each type of loop

  // get number of train
  int train_number = 0;
  Printf("Train: ");
  int c;
  while (TRUE) {
    c = Getc(COM2);
    if (c == '\r')
      break;
    if (c >= '0' && c <= '9') {
      Putc(COM2, c);
      train_number = train_number*10+(c-'0');
    }
  }
  Printf("\r\nTrain %d selected.\r\n", train_number);

  track sts;
  sensor last;
  char msg[10];
  SensorInit(&last, 'Z', 0, 0);

  // init train
  Tr(&sts, train_number, 0);

  // init switches
  for (i = 1; i <= 156; i++) {
    if (i == 19)  // skip non-existent switches
      i = 153;
    Sw(&sts, i, 'C');
  }
  Sw(&sts, 8, 'S');
  Sw(&sts, 9, 'S');
  Sw(&sts, 14, 'S');
  Sw(&sts, 15, 'S');

  // init timer 4
  int *timer4_hval = (int*)( TIMER4_HVAL );
  *timer4_hval = T4EN_MASK;

  // start sensor monitor
  int monitor_tid = Create(3, &SensorMonitor);
  Send(monitor_tid, NULL, 0, NULL, 0);
  
  // message to parent: we're starting!
  Send(0, NULL, 0, NULL, 0);

  // Phase 0: the starting gate
  Printf("Phase 0\r\n");
  Tr(&sts, train_number, 9);
  while (last.set != 'E' || last.id != 11) {
    Receive(&monitor_tid, msg, 10);
    Reply(monitor_tid, NULL, 0);
    FillSensorData(msg, &last);
    DataPrint(&sts, &last, train_number);
    PrintData(msg);
  }
  SensorInit(&last, 'Z', 0, 0);
  // Phase 1: N outer loops
  Printf("Phase 1\r\n");
  Sw(&sts, 6, 'S');
  Sw(&sts, 7, 'S');
  for (i = 0; i < N; i++) {
    Tr(&sts, train_number, N-i+3);
    while (last.set != 'E' || last.id != 11) {
      Receive(&monitor_tid, msg, 10);
      Reply(monitor_tid, NULL, 0);
      FillSensorData(msg, &last);
      DataPrint(&sts, &last, train_number);
      PrintData(msg);
    }
    // swap 6, 7 for branch at bottom
    Sw(&sts, 6, sts.sw_dir[6] == 'C' ? 'S' : 'C');
    Sw(&sts, 7, sts.sw_dir[7] == 'C' ? 'S' : 'C');
    SensorInit(&last, 'Z', 0, 0);
  }
  // Phase 2: N figure-eights
  Printf("Phase 2\r\n");
  Sw(&sts, 8, 'C');
  Sw(&sts, 9, 'C');
  Sw(&sts, 14, 'C');
  Sw(&sts, 15, 'C');
  for (i = 0; i < N; i++) {
    Tr(&sts, train_number, N-i+3);
    while (last.set != 'D' || last.id != 5) {
      Receive(&monitor_tid, msg, 10);
      Reply(monitor_tid, NULL, 0);
      FillSensorData(msg, &last);
      DataPrint(&sts, &last, train_number);
      PrintData(msg);
    }
    SensorInit(&last, 'Z', 0, 0);
    Sw(&sts, 153, 'C');
    Sw(&sts, 154, 'S');
    Sw(&sts, 155, 'C');
    Sw(&sts, 156, 'S');
    while (last.set != 'A' || last.id != 3) {
      Receive(&monitor_tid, msg, 10);
      Reply(monitor_tid, NULL, 0);
      FillSensorData(msg, &last);
      DataPrint(&sts, &last, train_number);
      PrintData(msg);
    }
    SensorInit(&last, 'Z', 0, 0);
    Sw(&sts, 153, 'S');
    Sw(&sts, 154, 'C');
    Sw(&sts, 155, 'S');
    Sw(&sts, 156, 'C');
  }
  while (last.set != 'E' || last.id != 9) {
    Receive(&monitor_tid, msg, 10);
    Reply(monitor_tid, NULL, 0);
    FillSensorData(msg, &last);
    DataPrint(&sts, &last, train_number);
    PrintData(msg);
  }
  SensorInit(&last, 'Z', 0, 0);
  // Phase 3: N inner loops
  Printf("Phase 3\r\n");
  Sw(&sts, 10, 'S');
  Sw(&sts, 13, 'S');
  Sw(&sts, 16, 'S');
  Sw(&sts, 17, 'S');
  for (i = 0; i < N; i++) {
    Tr(&sts, train_number, N-i+3);
    while (last.set != 'E' || last.id != 9) {
      Receive(&monitor_tid, msg, 10);
      Reply(monitor_tid, NULL, 0);
      FillSensorData(msg, &last);
      DataPrint(&sts, &last, train_number);
      PrintData(msg);
    }
    SensorInit(&last, 'Z', 0, 0);
  }
  // Phase 4: reverse N inner loops
  Printf("Phase 4\r\n");
  Rv(&sts, train_number);
  for (i = 0; i < N; i++) {
    Tr(&sts, train_number, N-i+3);
    while (last.set != 'D' || last.id != 6) {
      Receive(&monitor_tid, msg, 10);
      Reply(monitor_tid, NULL, 0);
      FillSensorData(msg, &last);
      DataPrint(&sts, &last, train_number);
      PrintData(msg);
    }
    SensorInit(&last, 'Z', 0, 0);
  }
  // Phase 5: reverse N figure-eights
  Printf("Phase 5\r\n");
  Sw(&sts, 10, 'C');
  Sw(&sts, 13, 'C');
  Sw(&sts, 16, 'C');
  Sw(&sts, 17, 'C');
  for (i = 0; i < N; i++) {
    Tr(&sts, train_number, N-i+3);
    while (last.set != 'E' || last.id != 10) {
      Receive(&monitor_tid, msg, 10);
      Reply(monitor_tid, NULL, 0);
      FillSensorData(msg, &last);
      DataPrint(&sts, &last, train_number);
      PrintData(msg);
    }
    SensorInit(&last, 'Z', 0, 0);
    Sw(&sts, 153, 'C');
    Sw(&sts, 154, 'S');
    Sw(&sts, 155, 'C');
    Sw(&sts, 156, 'S');
    while (last.set != 'B' || last.id != 16) {
      Receive(&monitor_tid, msg, 10);
      Reply(monitor_tid, NULL, 0);
      FillSensorData(msg, &last);
      DataPrint(&sts, &last, train_number);
      PrintData(msg);
    }
    SensorInit(&last, 'Z', 0, 0);
    Sw(&sts, 153, 'S');
    Sw(&sts, 154, 'C');
    Sw(&sts, 155, 'S');
    Sw(&sts, 156, 'C');
  }
  while (last.set != 'E' || last.id != 10) {
    Receive(&monitor_tid, msg, 10);
    Reply(monitor_tid, NULL, 0);
    FillSensorData(msg, &last);
    DataPrint(&sts, &last, train_number);
    PrintData(msg);
  }
  SensorInit(&last, 'Z', 0, 0);
  // Phase 6: reverse N outer loops
  Printf("Phase 6\r\n");
  Sw(&sts, 8, 'C');
  Sw(&sts, 9, 'C');
  Sw(&sts, 14, 'C');
  Sw(&sts, 15, 'C');
  for (i = 0; i < N; i++) {
    Tr(&sts, train_number, N-i+3);
    while (last.set != 'D' || last.id != 7) {
      Receive(&monitor_tid, msg, 10);
      Reply(monitor_tid, NULL, 0);
      FillSensorData(msg, &last);
      DataPrint(&sts, &last, train_number);
      PrintData(msg);
    }
    SensorInit(&last, 'Z', 0, 0);
    // swap 6, 7 for branch at bottom
    Sw(&sts, 6, sts.sw_dir[6] == 'C' ? 'S' : 'C');
    Sw(&sts, 7, sts.sw_dir[7] == 'C' ? 'S' : 'C');
  }
  // Phase 7: enter finishing section
  Printf("Phase 7\r\n");
  Sw(&sts, 6, 'C');
  Sw(&sts, 7, 'C');
  Sw(&sts, 18, 'S');
  while (last.set != 'A' || last.id != 6) {
    Receive(&monitor_tid, msg, 10);
    Reply(monitor_tid, NULL, 0);
    FillSensorData(msg, &last);
    DataPrint(&sts, &last, train_number);
    PrintData(msg);
  }
  SensorInit(&last, 'Z', 0, 0);
  // Phase 8: STOP!
  Printf("Phase 8\r\n");
  Tr(&sts, train_number, 0);
  // message to parent: we're done!
  Send(0, NULL, 0, NULL, 0);
  Exit();
}

void UserInit() {
  int child;
  child = Create(4, &NameServer);
  Send(child, NULL, 0, NULL, 0);
  child = Create(6, &SyncServer);
  Send(child, NULL, 0, NULL, 0);
  child = Create(6, &ClockServer);
  Send(child, NULL, 0, NULL, 0);
  child = Create(4, &TrainTxServer);
  Send(child, NULL, 0, NULL, 0);
  child = Create(5, &TrainRxServer);
  Send(child, NULL, 0, NULL, 0);
  child = Create(4, &TerminalRxServer);
  Send(child, NULL, 0, NULL, 0);
  child = Create(5, &TerminalTxServer);
  Send(child, NULL, 0, NULL, 0);
  
  Create(0, &IdleTask);
  child = Create(2, &TrainDriver);
  Receive(&child, NULL, 0);
  Reply(child, NULL, 0);
  
  Panic("UserInit", "This is a test panic attack!");
  
  Receive(&child, NULL, 0);
  Reply(child, NULL, 0);
  
  FingerOfDeath();
}
